
#include "Contact.h"

namespace p2 {

void Contact::initialise(Vector2<real>& pa, Vector2<real>& pb, uint32 pi)
{
	_pa[pi] = pa;
	_pb[pi] = pb;

	_dist[pi] = (_pb[pi] - _pa[pi]).dot(_normal);
	if (_dist[pi] > 0.f) {
		_dist[pi] = (_pb[pi] - _pa[pi]).length();
	}

	/*
	 * calculate radius arms
	 */
	_ra[pi] = (_pa[pi] - _a->getOwner()->getX()).perp();
	_rb[pi] = (_pb[pi] - _b->getOwner()->getX()).perp();

	_impulseN[pi] = _impulseT[pi] = 0;

	/*
	 * compute denominator in impulse equation
	 */
	real a = _a->getOwner()->getInvMass();
	real b = _b->getOwner()->getInvMass();
	real ran = _ra[pi].dot(_normal);
	real rbn = _rb[pi].dot(_normal);
	real c = ran * ran * _a->getOwner()->getInvInertia();
	real d = rbn * rbn * _b->getOwner()->getInvInertia();
	_invDenom[pi] = (real) 1 / (a + b + c + d);

	Vector2<real> t = _normal.perp();
	ran = _ra[pi].dot(t);
	rbn = _rb[pi].dot(t);
	c = ran * ran * _a->getOwner()->getInvInertia();
	d = rbn * rbn * _b->getOwner()->getInvInertia();
	_invDenomTan[pi] = (real) 1 / (a + b + c + d);
}

void Contact::applyImpulses(Vector2<real> impulse, uint32 pi)
{
	_a->getOwner()->incV(+_a->getOwner()->getInvMass() * impulse);
	_a->getOwner()->incW(+_a->getOwner()->getInvInertia() * impulse.dot(_ra[pi]));
	_b->getOwner()->incV(-_b->getOwner()->getInvMass() * impulse);
	_b->getOwner()->incW(-_b->getOwner()->getInvInertia() * impulse.dot(_rb[pi]));
}

}
